

//(C) 2011 Hannes Hassler. 
//May be used under 
//GNU GENERAL PUBLIC LICENSE (GPL3)
//http://www.gnu.org/licenses/gpl-3.0.html


#include "Wire.h"
#include <ctype.h>

#define bit9600Delay 84  
#define halfBit9600Delay 42
#define bit4800Delay 188
#define halfBit4800Delay 94

#define rstPin 16

#define NoNl     false
#define Nl       true

#define REFRESH_RATE 300


#define refreshCountdown 1000


int winkel;
int winkelAzimuth;
int winkelElevation;


      
char ch;


byte servoElevation=5;
byte servoAzimuth=4;
byte servoSpeed=3;

byte currentAzPos=0;
byte newAzPos=1;
byte currentElvPos=0;
byte newElvPos=1;

float k_az=0.65833;
int d_az=41;

float k_elv=0.6833;
int d_elv=130;

int refreshCountdownIndex=refreshCountdown;

boolean showServoResponse=false;
boolean showPololuReset=false;

unsigned long millisSinceLastServoRefresh=0;

byte debugShow=0;

void setup()
{
   
     
    pinMode(rstPin, OUTPUT);
    resetPololu();    
    
        
    Serial.begin(9600);
    Serial3.begin(57600);
    delay(500);
  
    //set_speed(servoElevation,servoSpeed);
    //set_speed(servoAzimuth,servoSpeed);
    
       
  
}



void loop()
{

 static int v = 0; 

 if (Serial.available()) { 
      ch = Serial.read();  
      switch(ch) {
     case '0'...'9':
        v = v * 10 + ch - '0';
        break;
      case 'E':
        show("elevation val:",v,Nl); 
        //set_speed(servoElevation,servoSpeed);  
        //maestro_set_target(servoElevation, v); 
        moveServo(servoElevation, v);    
        //position_8bit(servoElevation,v);       
        v = 0; break;
      case 'A':
        show("azimuthVal:",v,Nl);  
        //set_speed(servoAzimuth,servoSpeed);       
        //position_8bit(servoAzimuth,v); 
        maestro_set_target(servoAzimuth, v);
        v = 0; break;      
       v=0; break;         
      case 'q':
         showServoResponse=false;
         Serial.println("showServoResponse=false");
         break;
      case 'r':
         showServoResponse=true;
         Serial.println("showServoResponse=true");
         break;
      case 's':
         resetPololu();
         Serial.println("resetPololu");
         break;
      case 't':
         showPololuReset=true;
         Serial.println("showPololuReset=true");
         break;
      case 'u':
         showPololuReset=false;
         Serial.println("showPololuReset=false");
         break;
    }
  }  

  
}



void sendNum(int num) {
  Serial.print(num);  
}

void resetPololu() {
  
  digitalWrite(rstPin, LOW); //resets the Pololu board
  delay(100);
  digitalWrite(rstPin, HIGH);
  if (showPololuReset) {
    Serial.println("Pololu resetted");
  }
  
}

void show(char nameStr[], double val) {
  Serial.print(nameStr);  
  Serial.print("=");  
  Serial.print(val);
}

void show(char nameStr[], double val, boolean newline) {
  Serial.print(nameStr);  
  Serial.print("=");
  if (newline)
       Serial.println(val);
  else Serial.print(val);
}

void showln(char nameStr[], double val) {
  Serial.print(nameStr);  
  Serial.print("=");  
   Serial.println(val);
  
}

void showAzElv() {
  
   show("d_az:",d_az,Nl);
   show("d_elv:",d_elv,Nl);
   show("azPos:",currentAzPos,Nl);
   show("elvPos:",currentElvPos,Nl);
  
}


void moveServo(int ServoChannel, int target)
{
   //656ms PWM pulse represents a servo angle of 0 degrees.
   //2000ms PWM pulse represents a servo angele of 180 degrees.
   //These values could vary based on the servo you use, check your servo's 
   //spec documentation and verify what PWM pulses are needed to move it.
 
   byte serialBytes[4]; //Create the byte array object that will hold the communication packet.
 
   target = (map(target, 0, 180, 656, 2000)*4); //Map the target angle to the corresponding PWM pulse.
   show("target",target);
   showln("\tservo",ServoChannel);
 
   serialBytes[0] = 0x84; // Command byte: Set Target.
   serialBytes[1] = ServoChannel; // First byte holds channel number.
   serialBytes[2] = target & 0x7F; // Second byte holds the lower 7 bits of target.
   serialBytes[3] = (target >> 7) & 0x7F; // Third byte holds the bits 7-13 of target.
 
   Serial3.write(serialBytes, sizeof(serialBytes)); //Write the byte array to the serial port.
}


void position_8bit(byte servo, byte posValue)
{
  //this function uses pololu mode command 3 to set position
  //servo is the servo number (typically 0-7)
  //posValue * range (set with command 0) adjusted by neutral (set with command 5)
  //determines servo position

   byte temp;
   byte pos_hi,pos_low;
   int pos_hiInt, pos_lowInt;
   byte ch;
   int chInt;
   
   int sendTries=3;
   int noOfResponses[sendTries];
   

   temp = posValue & 0x80;	//get bit 8 of position
   pos_hi = temp >> 7;		//shift bit 8 by 7
   pos_hiInt=pos_hi;
   pos_low = posValue & 0x7f;     //get lower 7 bits of position
   pos_lowInt=pos_low;
   
   
   //Send a Pololu Protocol command
   if (showServoResponse) {
     Serial.print("send to servo:");
     Serial.print(pos_hiInt);
     Serial.print(" ");
     Serial.print(pos_lowInt);
     Serial.print("\n");
   }
   
   
   for (int i=0; i<sendTries;i++) {
     Serial3.write(0x80);     //start byte
     Serial3.write(0x01);     //device id
     Serial3.write(0x03);     //command number
     Serial3.write(servo);    //servo number
     Serial3.write(pos_hi);  //bits 8 thru 13
     Serial3.write(pos_low); //bottom 7 bits
   
     noOfResponses[i]=0;
     while (Serial3.available()) { 
        noOfResponses[i]++;
        ch = Serial3.read();
        chInt=ch;
        //if (chInt==0) {resetPololu();}
        if (showServoResponse) {
           Serial.print(chInt);
           Serial.print(" ");
        }
     }      
     if (showServoResponse) {
         Serial.print("\n");
     }
     if (chInt==pos_lowInt) {
         //Serial.print("OK\n\n");
     }
   }
   
   
   if (showServoResponse) {
         Serial.print("Sendtries: ");
   }
   boolean oneCorrectlyReceived=false;
   for (int i=0; i<sendTries;i++) {
     oneCorrectlyReceived=oneCorrectlyReceived || (noOfResponses[i]==6);
     if (showServoResponse) {
         Serial.print(noOfResponses[i]);
         Serial.print(" ");
     }
     
   }
   if (showServoResponse) {
         Serial.print("\n");
   }
   if (!oneCorrectlyReceived) {resetPololu();}
   
   
}

void set_speed(byte servo, byte speedVal)
{
   //this function uses pololu mode command 1 to set speed
   //servo is the servo number (typically 0-7)
   //speedVal is servo speed (1=slowest, 127=fastest, 0=full)
   //set speedVal to zero to turn off speed limiting

   speedVal = speedVal & 0x7f; //take only lower 7 bits of the speed

   //Send a Pololu Protocol command
   Serial3.write(0x80);     //start byte
   Serial3.write(0x01);     //device id
   Serial3.write(0x01);     //command number
   Serial3.write(servo);    //servo number
   Serial3.write(speedVal); //speed
}

void maestro_send_header(byte cmd, byte device){
  // sends the header of the command, depending ont the protocol
  
  if ( (cmd & 0x80) == 0 ) {   // Pololu protocol
    Serial3.write(0xAA);    //start byte
    Serial3.write(device);  //device
  }
  Serial3.write(cmd);
}

void MAESTRO_START_TRANSMISSION() {
}

void MAESTRO_END_TRANSMISSION() {
}


void maestro_send_command(byte cmd, byte device){
  // sends a command without data
  
  MAESTRO_START_TRANSMISSION();
  maestro_send_header(cmd, device);
  MAESTRO_END_TRANSMISSION();
}

void maestro_send_command(byte cmd, byte val1, byte device){
  // sends a command with 1 byte of data

  MAESTRO_START_TRANSMISSION();
  maestro_send_header(cmd, device);
  Serial3.write(val1);
  MAESTRO_END_TRANSMISSION();
}

void maestro_send_command(byte cmd, byte val1, int val2, byte device){
  // sends a command with 3 byte of data

  MAESTRO_START_TRANSMISSION();
  maestro_send_header(cmd, device);
  Serial3.write(val1);
  Serial3.write(val2 & 0x7f); //data lower 7 bits
  Serial3.write((val2 >> 7) & 0x7f); //data bits 7-13
  MAESTRO_END_TRANSMISSION();
}

int maestro_receive_int(){
  int low, high;
  
  while(!Serial3.available()); // wait until some data is present
  
  high = Serial3.read();       // read the first byte
  while(Serial3.available()){  // while the rx buffer is not empty
    low=high;                        // put the previous value read in "low" 
    high = Serial3.read();     // put the last value in "high"
  }
  // at this point, "high" contains the last value received, and "low" the one
  // before that. Any garbage that could have been in the rx buffer has been
  // effectively thrown away.
  
  return low | (high << 8);
}

byte maestro_receive_byte(){
  byte data;
  
  while(!Serial3.available()); // wait until some data is present
  
  while(Serial3.available()){  // while the rx buffer is not empty
    data = Serial3.read();     // put the last value in "data"
  }
  // at this point, "data" contains the last value received. Any garbage
  // that could have been in the rx buffer has been effectively thrown away.
  
  return data;
}

void maestro_set_target(byte servo, int angle){
  //set the target for a servo
  //servo is the servo number (typically 0-5)
  //angle is the target, from 256 to 13120, in quarter-microseconds
  
  maestro_send_command(0x84, servo, angle, 0);
}
